##Spawn and Delete(TODO) gazebo models according to /gazebo/set_model_states
import rospy
import time
#import pyquaternion
from gazebo_msgs.srv import SetModelState, SpawnModel
from gazebo_msgs.msg import ModelState,ModelStates
from math import pi, cos, sin
from geometry_msgs.msg import *

class spawn_model_group:

  def __init__(self):
    rospy.Timer(rospy.Duration(.5), self.timer_callback) # hz timer loop
    rospy.Subscriber('/gazebo/set_model_states', ModelStates, self.spawn_callback)
    self.spawn_model = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel)
    '''
    file = open("./SUV.sdf",encoding = "utf-8")
    self.car_sdf_xml = file.read()
    print('car_sdf_xml read!')
    file.close()

    file = open("./quadrotor.sdf",encoding = "utf-8")
    self.drone_sdf_xml = file.read()
    print('drone_sdf_xml read!')
    file.close()
    '''
    self.car_sdf_xml = rospy.get_param('~car_sdf')
    self.drone_sdf_xml = rospy.get_param('~drone_sdf')
    print('[Spawn_model_group] waiting for /gazebo/set_model_states message...')
    self.model_names = []
    self.msg_data = []
  
  def spawn_callback(self, msg):
    self.msg_data = msg.name

  def timer_callback(self,event):
    if self.msg_data != self.model_names:
      self.model_names = self.msg_data
      for num in range(len(self.model_names)):
        if "car" in self.model_names[num]:
          item_pose = Pose(Point(x=0, y=0, z=0),  Quaternion(0, 0, 0, 1))
          self.spawn_model(self.model_names[num], self.car_sdf_xml, "", item_pose, "world")
          time.sleep(0.2)
          print('spawn ',self.model_names[num],' successfully!')
        elif "drone" in self.model_names[num]:
          item_pose = Pose(Point(x=0, y=0, z=3),  Quaternion(0, 0, 0, 1))
          self.spawn_model(self.model_names[num], self.drone_sdf_xml, "", item_pose, "world")
          time.sleep(0.2)
          print('spawn ',self.model_names[num],' successfully!')
        else:
          print('[Spawn_model_group] wrong model names! model names should contain "car" or "drone"!')  

if __name__ == '__main__':
    rospy.init_node("spawn_model_group")
    run_node = spawn_model_group()
    rospy.spin()